import abc
import threading
import time
from abc import abstractmethod

import numpy as np

from structures.basic_structure import BasicStructure
from utils.math_tool import calc_slice_count


class BaseTrajectory(metaclass=abc.ABCMeta):

    def __init__(self):
        self.is_running = False
        self._args = None
        self.is_walking_completed = False
        self.path_points = []
        self.path_angles = []

    def init_args(self, args):
        if args is not None:
            self._args = args
            return

        self._args = []

    def get_path_angles(self):
        return self.path_angles

    def get_path_points(self):
        return self.path_points

    def tracking(self, args, update_cb, ls: BasicStructure):

        if args is None or len(args) == 0:
            return

        key_points = ls.forward_ratio(np.array(args))

        for i in range(len(key_points) - 1):
            start_point = key_points[i]
            end_point = key_points[i + 1]

            self._track_line_points(start_point, end_point, update_cb, ls)

        self._track_line_points(key_points[-1], key_points[0], update_cb, ls)

    @abstractmethod
    def get_track_key(self):
        pass

    def get_color(self):
        return [0, 0, 0]

    def run(self, update_cb, ls: BasicStructure):
        if self.is_running:
            print("----- walking... -----")
            return

        threading.Thread(target=self.start_walks, args=(update_cb, ls)).start()

    def save(self, ls):
        ls.get_trajectory_list().append(self)

    def clean(self):
        self.path_points.clear()
        self.path_angles.clear()

    def start_walks(self, update_cb, ls):
        self.is_running = True

        self.tracking(self._args, update_cb, ls)

        self.is_running = False

        # 表示已经进行过一次路径规划，不再进行路径规划，以后只执行绘制不添加新的点
        self.is_walking_completed = True

    def _add_point(self, p):

        if self.is_walking_completed:
            return

        if isinstance(p, np.ndarray):
            p = p.tolist()

        # p = [round(v, 1) for v in p]

        self.path_points.append(p)

    def _add_angles(self, angles):
        if self.is_walking_completed:
            return

        if isinstance(angles, np.ndarray):
            angles = angles.tolist()

        self.path_angles.append(angles)

    def _track_line_points(self, start_point, end_point, update_cb, ls):
        if isinstance(start_point, list):
            start_point = np.array(start_point)
        if isinstance(end_point, list):
            end_point = np.array(end_point)

        s_count = calc_slice_count(np.linalg.norm(end_point - start_point))
        slice_count = np.max([s_count, 2])

        slice_time_second = (1 / slice_count) * 0.1
        print(f"line -> slice_count: {slice_count} sleep: {slice_time_second}")
        linspace_points = np.linspace(start_point, end_point, num=slice_count)
        for p in linspace_points:
            if ls.try_update_point_end(p):
                alpha, beta = ls.inverse_not_update(p)
                self._add_angles((alpha, beta))
                self._add_point(p)
                update_cb()
                time.sleep(slice_time_second)

